#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include "tf2/LinearMath/Quaternion.h"
#include<geometry_msgs/Quaternion.h>
#include<geometry_msgs/PoseStamped.h>
#include<tf/tf.h>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include "iostream"
#include "algorithm"
#include "cmath"
using namespace std;
ros::Publisher pub;
ros::Subscriber sub;


typedef struct  p_robot
{
    Eigen::Vector3d Pose;
    Eigen::Matrix3d Rotation;

    double theat;
}P_robot;

P_robot p_start;
P_robot p_now;

    //给定目标点
    double goal_x;
    double goal_y;  
    double goal_theat;
    double dtheat;
    bool isstart;
    bool flag_theat=true;
    bool get_point=true;
    bool flag_front=true;
    bool startornot;
    double  goal_theat_start;
    double goal_odom_x;
    double goal_odom_y;
    double d_x;
    double d_y;
    double p1;
    double p2;
    double max_dis;
    bool flag_goal=true;

void odomCallback(const nav_msgs::OdometryConstPtr& odom)
{   
    p_now.Rotation = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
                                          odom->pose.pose.orientation.y, odom->pose.pose.orientation.z).toRotationMatrix();
    double roll, pitch, yaw;
    tf::Quaternion q;
    tf::quaternionMsgToTF(odom->pose.pose.orientation,q);
    tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
    p_now.theat=yaw;
    cout<<"yaw  is: "<<p_now.theat<<endl;
    p_now.Pose(0) = odom->pose.pose.position.x;
    p_now.Pose(1) = odom->pose.pose.position.y;
    p_now.Pose(2) = odom->pose.pose.position.z;
    cout<<"x,y is: "<<p_now.Pose(0)<<" ,"<<p_now.Pose(1)<<endl;

      if(get_point==true)
    {
        p_start.Rotation = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
                                     odom->pose.pose.orientation.y, odom->pose.pose.orientation.z).toRotationMatrix();
        double roll, pitch, yaw;
        tf::Quaternion q;
        tf::quaternionMsgToTF(odom->pose.pose.orientation,q);
        tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
        p_start.theat=yaw;
        p_start.Pose(0) = odom->pose.pose.position.x;
        p_start.Pose(1) = odom->pose.pose.position.y;
        p_start.Pose(2) = odom->pose.pose.position.z;
        p1=p_start.Pose(0);
        p2=p_start.Pose(1);
        max_dis=sqrt(pow(p1-goal_x-p1,2)+pow(p2-goal_y-p2,2));
        goal_theat=atan2(goal_y-p_start.Pose(1),goal_x-p_start.Pose(0));
        get_point=false;
    }
      geometry_msgs::Twist twist;
    // 运动距离超过最大距离直接停止
    //    double distance=sqrt(pow(p1-p_now.Pose(0),2)+pow(p2-p_now.Pose(1),2));
    //    if(distance>max_dis){
    //     twist.angular.z=0;
    //     twist.linear.x=0;
    //     pub.publish(twist);
    //     cout<<"-------超过最大距离----------"<<endl;
    //     return ;
    // }
    double p_theat=atan2(goal_y-p_now.Pose(1),goal_x-p_now.Pose(0));
    dtheat=p_theat-p_now.theat;
    if(dtheat>3.14){
        dtheat=dtheat-6.28;
    }
    if(dtheat<-3.14){
        dtheat=dtheat+6.28;
    }
    cout<<"-------dtheat--------   "<<dtheat<<endl;
    
    if(fabs(dtheat)>0.01&&flag_theat==true)
    {
        twist.linear.x=0.2;
        twist.angular.z=0.8*dtheat;
    cout<<"-------vx,vz--------   "<<twist.linear.x<<"  "<<twist.angular.z<<endl;

        if(fabs(twist.angular.z)>0.5){
            if (dtheat>0.0)
            {
                twist.angular.z=0.5;
                twist.linear.x=0.1;
            }
            else
            {
                twist.angular.z=-0.5;
                twist.linear.x=0.1;
            }
        }
        d_x=fabs(goal_x-p_now.Pose(0));
        d_y=fabs(goal_y-p_now.Pose(1));
    cout<<"-------dx--------   "<<d_x<< endl;
    cout<<"-------dy--------   "<<d_y<<endl;
         if(d_x<0.05&&d_y<0.05)
    {
        twist.angular.z=0.0;
        twist.linear.x=0.0;
        pub.publish(twist);

        cout<<"-----到达目标点------"<<endl;
        flag_goal=false;
    }
    cout<<"-------flag_goal-------"<<flag_goal<<endl;
    if (flag_goal)
    {
        cout<<"----发布消息----"<<endl;
         pub.publish(twist);
    }
        return;
    }
    else{

         twist.angular.z=0.1*dtheat;
        twist.linear.x=0.1;
        pub.publish(twist);
        d_x=fabs(goal_x-p_now.Pose(0));
        d_y=fabs(goal_y-p_now.Pose(1));
    cout<<"-------dx--------   "<<d_x<< endl;
    cout<<"-------dy--------   "<<d_y<<endl;
         if(d_x<0.05&&d_y<0.05)
    {
        twist.angular.z=0.0;
        twist.linear.x=0.0;
        pub.publish(twist);

        cout<<"-----到达目标点------"<<endl;
        flag_goal=false;
    }


//         d_x=fabs(goal_x-p_now.Pose(0));
//         d_y=fabs(goal_y-p_now.Pose(1));
//         double dis=sqrt(pow(d_x,2)+pow(d_y,2));
//          if((d_x<0.05&&d_y<0.05)||(dis<0.1))
//     {
//         cout<<"-------dx--------   "<<d_x<< endl;
//         cout<<"-------dy--------   "<<d_y<<endl;
//         twist.angular.z=0.0;
//         twist.linear.x=0.0;
// }
}
}

int main(int argc, char *argv[])
{ 
    cout<<"请输入X方向的位置：";
    cin>>goal_x;
    cout<<"请输入Y方向的位置：";
    cin>>goal_y;
     setlocale(LC_ALL,"");
    ros::init(argc,argv,"test_point");
    ros::NodeHandle nh;

    //发布速度控制消息
     pub=nh.advertise<geometry_msgs::Twist>("/cmd_vel",50);
      sub=nh.subscribe("/t265/odom/sample",100,odomCallback);
 
    ros::spin();
    return 0;
}
